#include "swarmBotOS.hpp"

inline string initializeAlgorithms(LED *red, LED *green, LED *blue, Motor *motor, Bumper *bump, Finder *finder, map<string, SwarmAlgorithm *> &algorithms, vector<vector<string> > *algMessages) {
    SwarmAlgorithm::loadControls(red, green, blue, motor, bump, finder, algMessages);

    string names = "";
    map<string, SwarmAlgorithm *>::iterator it = algorithms.begin();
    
    // To add an SwarmAlgorithm:
    //  SwarmAlgorithm *<algObj> = new <AlgClass>(<extra parameters>);
    //  string <algName> = <algObj>->getID();
    //  names += <algName> + ","; // don't include '+ ","' for last SwarmAlgorithm
    //  algorithms.insert(it, pair<string, SwarmAlgorithm *>(<algName>, <algObj>));
    
    SwarmAlgorithm *cluster = new ClusterAlgorithm();
    string clusterName = cluster->getID();
    names += clusterName + ",";
    algorithms.insert(it, pair<string, SwarmAlgorithm *>(clusterName, cluster));
    
    SwarmAlgorithm *groupByColor = new GroupByColorAlgorithm();
    string groupByColorName = groupByColor->getID();
    names += groupByColorName + ",";
    algorithms.insert(it, pair<string, SwarmAlgorithm *>(groupByColorName, groupByColor));
    
    SwarmAlgorithm *followLeader = new FLAlgorithm();
    string followLeaderName = followLeader->getID();
    names += followLeaderName;
    algorithms.insert(it, pair<string, SwarmAlgorithm *>(followLeaderName, followLeader));
    return names;
}

inline void sendDebugInfo(LED *red, LED *green, LED *blue, Motor *motor, Bumper *bump, Finder *finder) {
    SwarmOS::writeMessage(SwarmOS::HOST_ID, SwarmOS::DEBUG_HDR);

    // peripheral information
    string peripheralMessage = "PERIPHERALS: RED ";
    peripheralMessage += red->getIsOn() ? "ON" : "OFF";
    peripheralMessage += " GREEN ";
    peripheralMessage += green->getIsOn() ? "ON" : "OFF";
    peripheralMessage += " BLUE ";
    peripheralMessage += blue->getIsOn() ? "ON" : "OFF";
    peripheralMessage += " RIGHT ";
    peripheralMessage += itoa(motor->getLastRightSpeed());
    peripheralMessage += " LEFT ";
    peripheralMessage += itoa(motor->getLastLeftSpeed());
    peripheralMessage += " FRONT ";
    peripheralMessage += bump->isFrontTouching() ? "ON" : "OFF";
    peripheralMessage += " BACK ";
    peripheralMessage += bump->isBackTouching() ? "ON" : "OFF";

    SwarmOS::writeMessage(SwarmOS::HOST_ID, peripheralMessage);

    // robot locations
    string robotsMessage = "ROBOTS: ";
    vector<Robot> robots = finder->getRobotPositions();
    for (int i = 0; i < robots.size(); i++) {
        Robot r = robots[i];
        PolarCoordinate pc = r.position;
        robotsMessage += ftoa(pc.radius_cm);
        robotsMessage += " ";
        robotsMessage += ftoa(pc.angle_rad);
        robotsMessage += " ";
        robotsMessage += r.label;
        robotsMessage += " ";
    }

    SwarmOS::writeMessage(SwarmOS::HOST_ID, robotsMessage);

    // robot connections
    string conMessage = "CONNECTIONS:";
    int size = SwarmOS::getSwarmSize();
    for (int i = 1; i < size; i++) {
        conMessage += " ";
        conMessage += SwarmOS::getIP(i);
        conMessage += " ";
        conMessage += SwarmOS::getIsConnected(i) ? "ON" : "OFF";
    }

    SwarmOS::writeMessage(SwarmOS::HOST_ID, conMessage);
}

inline void sendDebugImages(Finder *finder) {
    SwarmOS::writeMessage(SwarmOS::HOST_ID, SwarmOS::DEBUG_IMG_HDR);

    vector<Robot> robots = finder->getRobotPositions();

    // current image (BGR)
    Mat currImg = finder->getCurrentImage();
    list<string> currImgStrList = cvMatToStr(currImg, "BGR");
    for (list<string>::iterator it = currImgStrList.begin(); it != currImgStrList.end(); it++) {
        SwarmOS::writeMessage(SwarmOS::HOST_ID, *it);
    }

    // binary images (blobs)
    vector<Mat> binImgs = finder->getBinaryImages();

    for (int i = 0; i < binImgs.size(); i++) {
        list<string> binImgStrList = cvMatToStr(binImgs[i], "GRAY");
        for (list<string>::iterator it = binImgStrList.begin(); it != binImgStrList.end(); it++) {
            SwarmOS::writeMessage(SwarmOS::HOST_ID, *it);
        }
    }
}

int main(int argc, char * argv[] )
{
    bool SPIError = SPIInit() == SPI_SETUP_ERROR;

    LED green(LED::GREEN), red(LED::RED), blue(LED::BLUE);
    Motor motor;
    Bumper bump;
    Finder finder;

    vector<vector<string> > swarmOSMessages;
    vector<vector<string> > algorithmMessages;

    SwarmOS::useNetwork(SwarmOS::CONSTRUCT, &swarmOSMessages, &algorithmMessages);

    map<string, SwarmAlgorithm *> algorithms;
    string algNames = initializeAlgorithms(&red, &green, &blue, &motor, &bump, &finder, algorithms, &algorithmMessages);
    SwarmOS::writeMessage(SwarmOS::HOST_ID, algNames);

    if (SPIError) {
        SwarmOS::writeMessage(SwarmOS::HOST_ID, "Error:  SPI Setup failed.  Try manual reset (attach PICKIT to PIC programming header or remove PIC power).  This OS will resume after manual reset.");
        SPIInit(); // hang until manual reset
    }    
    printf("SPI Setup succeeded\n");
    
    SwarmAlgorithm * currentAlgorithm = NULL;

    string currentAlgorithmName = "",
           direction = "";

    bool debugging = false,
         isDoneSent = true; // whether or not to tell debugging Host we are done talking

    int resumeCount = -1,
        speed = 0;

    vector<string> debugInst; // for programmatically re-entering debug after a finite RESUME
    debugInst.push_back(SwarmOS::DEBUG);

    while(true)
    {
        // Make sure to grab all new messages to OS        
        SwarmOS::refreshNetworkMessages();

        // handle end of finite RESUME
        // finite RESUMEs resume algorithm for <resumeCount> OS execution
        // frames.  When done, the robot should stop and send debug data.
        if (resumeCount >= 0) {
            resumeCount--;
            if (resumeCount == -1) {                
                swarmOSMessages.insert(swarmOSMessages.begin(), debugInst);
            }
        }

        // Parse messages
        while(!swarmOSMessages.empty())
        {
            isDoneSent = false; // we now will need to send DONE signal

            // Load into String Stream for processing
            string swarmOSMessage = swarmOSMessages.front()[0];
            stringstream messageStream(swarmOSMessage);
            swarmOSMessages.erase(swarmOSMessages.begin());
            string command = "";
            if (messageStream.eof()) {
                continue;
            }
            messageStream >> command;

            // Handle Start algorithm for switching and nonswitching
            if(algorithms.count(command) > 0) {
                if (currentAlgorithm != NULL)
                {
                
                    currentAlgorithmName = command;                
                    currentAlgorithm = algorithms[command];
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, "Switching from " + 
                        currentAlgorithmName + " to " + command);
                    currentAlgorithm->setup();
                
                }
                else
                {
                    currentAlgorithmName = command;                
                    currentAlgorithm = algorithms[command];
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, "Running " + command);
                    currentAlgorithm->setup();
                }
            }
            // Handle debug info command
            else if (command == SwarmOS::DEBUG) {
                debugging = true;
                sendDebugInfo(&red, &green, &blue, &motor, &bump, &finder);
                motor.setReportSpeed(false);
            }
            // Handle debug img command
            else if (command == SwarmOS::DEBUGIMG) {
                debugging = true;
                motor.setReportSpeed(true);
                sendDebugImages(&finder);
            }
            else if (command == SwarmOS::RESUME) {
                if (!messageStream.eof()) {
                    messageStream >> resumeCount;
                }
                else {
                    resumeCount = -1;
                }
                
                motor.setReportSpeed(false);
                debugging = false;
            }
            // Handle raw SPI
            else if (command == SwarmOS::RAW) {
                unsigned char data;
                if (!messageStream.eof()) {
                    int temp;
                    messageStream >> hex >> temp;
                    data = (unsigned char) temp;
                }

                send(data);
            }
            // Handle peripheral getter
            else if (command == SwarmOS::GET) {
                string periError = "Error:  expected peripheral name after GET (RED, GREEN, BLUE, LEFT, RIGHT, FRONT, BACK)";
                string dataError = "Error:  expected hex data after ";

                string periName;
                if (messageStream.eof()) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, periError);    
                    continue;
                }
                messageStream >> periName;
                
                if (periName == SwarmOS::RED) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, red.getIsOn() ? "ON" : "OFF");    
                }
                else if (periName == SwarmOS::GREEN) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, green.getIsOn() ? "ON" : "OFF");    
                }
                else if (periName == SwarmOS::BLUE) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, blue.getIsOn() ? "ON" : "OFF");    
                }
                else if (periName == SwarmOS::RIGHT) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, itoa(motor.getLastRightSpeed()));    
                }
                else if (periName == SwarmOS::LEFT) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, itoa(motor.getLastLeftSpeed()));    
                }
                else if (periName == SwarmOS::FRONT) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, bump.isFrontTouching() ? "ON" : "OFF");
                }
                else if (periName == SwarmOS::BACK) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, bump.isBackTouching() ? "ON" : "OFF");
                }
                else {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, periError);    
                }
            }
            // Handle peripheral setter
            else if (command == SwarmOS::SET) {
                string periError = "Error:  expected peripheral name after SET (RED, GREEN, BLUE, LEFT, RIGHT)";
                string dataError = "Error:  expected hex data after ";

                string periName;
                if (messageStream.eof()) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, periError);    
                    continue;
                }
                messageStream >> periName;
                
                unsigned char data;
                int temp;
                if (messageStream.eof()) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, dataError + periName);    
                    continue;
                }
                messageStream >> hex >> temp;
                data = (unsigned char) temp;
                
                if (periName == SwarmOS::RED) {
                    red.setLED(data != 0);
                }
                else if (periName == SwarmOS::GREEN) {
                    green.setLED(data != 0);
                }
                else if (periName == SwarmOS::BLUE) {
                    blue.setLED(data != 0);
                }
                else if (periName == SwarmOS::RIGHT) {
                    motor.rightWheelVar(data);    
                }
                else if (periName == SwarmOS::LEFT) {
                    motor.leftWheelVar(data);    
                }
                else {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, periError);    
                }
            }
            // Handle direction commands
            else if (command == SwarmOS::FORWARD || command == SwarmOS::FD ||
                     command == SwarmOS::BACK    || command == SwarmOS::BK ||
                     command == SwarmOS::RIGHT   || command == SwarmOS::RT ||
                     command == SwarmOS::LEFT    || command == SwarmOS::LT ||
                     command == SwarmOS::SPIN    || command == SwarmOS::TURN) {

                direction = command;

                // require RIGHT or LEFT if command is SPIN or TURN
                if (command == SwarmOS::SPIN || command == SwarmOS::TURN) {
                    string temp = "EOF";
                    if (!messageStream.eof()) {
                        messageStream >> temp;
                    }
                    if (temp == SwarmOS::RT) {
                        temp = SwarmOS::RIGHT;
                    }
                    else if (temp == SwarmOS::LT) {
                        temp = SwarmOS::LEFT;
                    }
                    if (temp != SwarmOS::RIGHT && temp != SwarmOS::LEFT) {
                        SwarmOS::writeMessage(SwarmOS::HOST_ID, "Error:  " 
                            + command + " expected " + SwarmOS::RIGHT
                            + " or " + SwarmOS::LEFT + "; got " + temp);
                        continue;
                    }
                    direction += temp;
                }

                if (!messageStream.eof()) {

                    // require numeric speed
                    string temp;
                    messageStream >> temp;
                    if (sscanf(temp.c_str(), "%d", &speed) != 1) {
                        speed = -1;
                        SwarmOS::writeMessage(SwarmOS::HOST_ID, "Error:  " 
                            + command + " expected speed number; got " + direction);
                        continue;
                    }
                }
                else {
                    speed = DEFAULT_SPEED;
                }

                if (direction == SwarmOS::FORWARD || direction == SwarmOS::FD) {
                    motor.goOffset(speed);
                }
                else if (direction == SwarmOS::BACK || direction == SwarmOS::BK) {
                    motor.reverseOffset(speed);
                }
                else if (direction == SwarmOS::RIGHT || direction == SwarmOS::RT || direction == SwarmOS::SPIN + SwarmOS::RIGHT) {
                    motor.rightWheelReverseOffset(speed);
                    motor.leftWheelGoOffset(speed);
                }
                else if (direction == SwarmOS::LEFT || direction == SwarmOS::LT || direction == SwarmOS::SPIN + SwarmOS::LEFT) {
                    motor.rightWheelGoOffset(speed);
                    motor.leftWheelReverseOffset(speed);
                }
                else if (direction == SwarmOS::TURN + SwarmOS::RIGHT) {
                    motor.rightWheelStop();
                    motor.leftWheelGoOffset(speed);
                }
                else if (direction == SwarmOS::TURN + SwarmOS::LEFT) {
                    motor.rightWheelGoOffset(speed);
                    motor.leftWheelStop();
                }
            }
            // Handle motor halt command
            else if (command == SwarmOS::STOP) {
                motor.stop();
            }
            // Handle reconnect command
            else if (command == SwarmOS::RECONNECT) {
                SwarmOS::useNetwork(SwarmOS::RECONN);
            }
            // Handle SPI reset command
            else if (command == SwarmOS::RESPI) {
                unsigned char data;
                if (!messageStream.eof()) {
                    messageStream >> data;
                    resetSPI(data);
                }
                else {
                    resetSPI();
                }
            }
            // Handle relay command
            // If from Host, in-message robot ID is different; relay to that ID
            // If from robot, relayed to this ID; perform in-message command or 
            //   echo non-command to host
            else if (command == SwarmOS::RELAY) {

                // require recipient id
                int id;
                if (messageStream.eof()) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, "Error:  expected RELAY robot ID and command text (OS:RELAY <id> <cmd>)");
                    continue;
                }

                // relay to recipient if id different than this robot's id
                messageStream >> id;
                if (id != SwarmOS::useNetwork(SwarmOS::GET_ID)) {
                    SwarmOS::writeMessage(id, SwarmOS::OS_INSTRUCTION+swarmOSMessage);
                    continue;
                }

                // else execute/echo (this robot is recipient).  Require instruction
                string inst;
                if (messageStream.eof()) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, "Error:  expected RELAY command text (OS:RELAY <id> <cmd>)");
                    continue;
                }
                getline(messageStream, inst);
                while (inst[0] == ' ') {
                    inst = inst.substr(1);
                }

                // if OS instruction, execute
                int osl = SwarmOS::OS_INSTRUCTION.length();
                if (inst.length() > osl && inst.substr(0, osl) == SwarmOS::OS_INSTRUCTION) {
                    vector<string> relayInst; // programmatically add OS inst
                    relayInst.push_back(inst.substr(osl));
                    swarmOSMessages.insert(swarmOSMessages.begin(), relayInst);
                    continue;
                }

                // else echo to Host
                SwarmOS::writeMessage(SwarmOS::HOST_ID, inst);    
            }
            // Handle stop command
            else if (command == SwarmOS::STOP) {
                if (currentAlgorithm != NULL) {
                    SwarmOS::writeMessage(SwarmOS::HOST_ID, "Stopping " + currentAlgorithmName);
                    currentAlgorithm = NULL;
                    currentAlgorithmName = "";
                }
                SwarmAlgorithm::turnLEDsOff();
                motor.stop();
            }
            // Otherwise the instruction was invalid and you should inform the SwarmHost
            else {
                SwarmOS::writeMessage(SwarmOS::HOST_ID, "Invalid instruction " 
                    + swarmOSMessage);    
            }

        }

        if (!isDoneSent) {
            // Tell debugging Host we are done talking (see DebugWindow::on_sendCommandButton_clicked())
            SwarmOS::writeMessage(SwarmOS::HOST_ID, SwarmOS::NET_DATA_DONE);
            isDoneSent = true; // OS should not send anything else until commanded again!
        }
        if (currentAlgorithm != NULL && currentAlgorithm->getIsRunning() && debugging){
            // Run algorithm
            currentAlgorithm->loopBody();
        }
    }
    return 0;
}



int SwarmOS::useNetwork(
    int function,
    vector<vector<string> > * swarmOSMessages,
    vector<vector<string> > * algorithmMessages, 
    string message, string *ip,int botnum){
    // TODO HANDLE ERRORS; part. Not connected errors
    // Static containers and network objects to access later
    static Network * net; // Default = NULL
    static vector<vector<string> > * swarmOSMessages_; // Default = NULL
    static vector<vector<string> > * algorithmMessages_; // Default = NULL
    
    switch (function) {
        case CONSTRUCT:
            // Behaves as constructor
            // swarmOSMessages != NULL && algorithmMessages != NULL && net == NULL
            net = new Network();        
            net->waitForHost(SwarmOS::OS_INSTRUCTION + SwarmOS::NET_DATA_ID, SwarmOS::OS_INSTRUCTION + SwarmOS::NET_DATA_IP, SwarmOS::OS_INSTRUCTION + SwarmOS::NET_DATA_IP_SIZE);
            swarmOSMessages_ = swarmOSMessages;
            algorithmMessages_ = algorithmMessages;
            break;
        case DESTRUCT:
            // Behaves as destructor    
            if(net != NULL){
                delete net;
                net = NULL;
                swarmOSMessages_ = NULL;
                algorithmMessages_ = NULL;
            }
            break;
        case REASSIGN:
            // Allows user to reassign messaging object
            // swarmOSMessages != NULL && algorithmMessages != NULL
            swarmOSMessages_ = swarmOSMessages;
            algorithmMessages_ = algorithmMessages;
            break;
        case WRITE:
            // write message
            // (botnum > -1 || botnum = -1 && ip != "") && !message.empty()
            if (botnum == -1){        
                return net->writeMessage(*ip, message);
            }
            else{
                return net->writeMessage(botnum, message);
            }
            // Handle errors        
            break;
        case BROADCAST:
            // broadcast message
            // !message.empty()
            net->broadcast(message);
            break;
        case RECONN:
            net->refresh(SwarmOS::OS_INSTRUCTION + SwarmOS::NET_DATA_ACK);
        case REFRESH:
            { // need these braces to not have a "crosses initialization error"
              // with messages below!
              // http://stackoverflow.com/questions/11578936/getting-a-bunch-of-crosses-initialization-error

                // Allows user to refresh network messages
                vector<vector<string> > messages = net->receiveMessage();
                for (int i = 0; i < messages.size(); i++){
                    string message = toupper(messages[i][0]);
                    int OSInstLen = SwarmOS::OS_INSTRUCTION.size();

                    // If it is a OS instruction the os will handle it
                    // (skip OS NET instructions, already handled by Network)
                    // otherwise pass it to the algorithm            
                    if (message.find(SwarmOS::OS_INSTRUCTION) == 0)
                    {
                        if (message.find(SwarmOS::NET_DATA) != OSInstLen) {
                            messages[i][0] = message.substr(OSInstLen);
                            swarmOSMessages_->push_back(messages[i]);
                        }
                    }
                    else
                    {
                        int AlgInstLen = SwarmOS::AL_INSTRUCTION.size();
                        if (message.find(SwarmOS::AL_INSTRUCTION) == 0)
                        {
                            message = message.substr(AlgInstLen);
                        }
                        messages[i][0] = message;
                        algorithmMessages_->push_back(messages[i]);            
                    }
                }
            }
            break;
        case GET_SIZE:
            // Allows user to get swarm size
            return net->size();
            break;
        case GET_ID:
            // Allows user to get this robot's swarm ID
            return net->getID();
            break;
        case GET_IP:
            // Allows user to get (this) robot's swarm ID
            if (botnum >= 0) {
                *ip = net->getIP(botnum);
            }
            else {
                *ip = net->getIP();
            }
            break;
        case GET_IS_CON:
            // Allows user to get botNum's connection status
            return net->isConnected(botnum);
            break;
        default:
            // Handle errors
            break;
    }

    return 0;
}
